#ifndef FOURMOTORDRIVESUBSYSTEM_H
#define FOURMOTORDRIVESUBSYSTEM_H

// Include files.
#include <map>
#include <boost/smart_ptr/shared_ptr.hpp>
#include "WPILib.h"
#include "../Services/JoystickDriveService.hpp"
#include "../Services/TwoJoystickDriveService.hpp"
#include "DriveSubsystem.hpp"
#include "../Values/TankValue.hpp"
#include "../Modifier.hpp"
#include "../Filters/Drive/ArcadeDriveFilter.hpp"
#include "../Filters/Drive/TankDriveFilter.hpp"
#include "../Operation.hpp"

/**
 *  The FourMotorTankDriveSubsystem class is a base class for all four motor drive subsystems.
 */
class TankDriveSubsystem :
	public DriveSubsystem
{
		friend class MecanumDriveSubsystem;
	public:
		enum Side
		{
			Left = 1,
			Right = 2
		};
		
		/**
		 *  The MotorPosition struct can be used to select drive motors.
		 */
		struct MotorPosition
		{
			friend class TankDriveSubsystem;
			
			public:
				MotorPosition(Side side, UINT32 rank = 0);
				
				bool operator<(MotorPosition other) const;
				
				inline Side GetSide() const
				{
					return m_side;
				}
				
				/**
				 *  Gets the rank of the motor position.  (All the way at the front is rank 0.)
				 */
				inline UINT32 GetRank() const
				{
					return m_rank;
				}
			private:
				Side m_side;
				UINT32 m_rank;
				
				RobotDrive::MotorType ToWpiValue(bool sideOnly = false);
		};
		
		static TankValue TankDriveFetchValue
		(
			GenericHID &leftJoystick, 
			GenericHID &rightJoystick
		);
		
		static TankValue ArcadeDriveFetchValue
		(
			GenericHID &joystick
		);
		
		static void DriveByTank(TankDriveSubsystem &drive, const TankValue &value);
		
		class ScaleModifier :
			public Modifier<TankValue>
		{
			public:
				ScaleModifier(double scale);
				virtual void Modify(TankValue &value);
			private:
				double m_scale;
		};
		
		class DeadZoneModifier :
			public Modifier<TankValue>
		{
			public:
				DeadZoneModifier(double minimumAbsValue);
				virtual void Modify(TankValue &value);
			private:
				double m_minimumAbsValue;
		};
		
		class ReverseModifier :
			public Modifier<TankValue>
		{
			public:
				virtual void Modify(TankValue &value);
		};
		
		TankDriveSubsystem
		(
			UINT32 leftFrontMotorChannel, 
			UINT32 leftBackMotorChannel,
			UINT32 rightFrontMotorChannel, 
			UINT32 rightBackMotorChannel
		);
		
		TankDriveSubsystem
		(
			UINT32 leftMotorChannel, 
			UINT32 rightMotorChannel
		);
		
		TankDriveSubsystem
		(
			boost::shared_ptr<PidMotorController> leftFrontMotor, 
			boost::shared_ptr<PidMotorController> leftBackMotor,
			boost::shared_ptr<PidMotorController> rightFrontMotor, 
			boost::shared_ptr<PidMotorController> rightBackMotor
		);
		
		TankDriveSubsystem
		(
			boost::shared_ptr<PidMotorController> leftMotor,
			boost::shared_ptr<PidMotorController> rightMotor
		);
		
		TankDriveSubsystem
		(
			PidMotorController *leftFrontMotor, 
			PidMotorController *leftBackMotor,
			PidMotorController *rightFrontMotor, 
			PidMotorController *rightBackMotor,
			bool canDeleteMotors = false
		);
		
		TankDriveSubsystem
		(
			PidMotorController *leftMotor,
			PidMotorController *rightMotor,
			bool canDeleteMotors = false
		);
		
		TankDriveSubsystem
		(
			PidMotorController &leftFrontMotor, 
			PidMotorController &leftBackMotor,
			PidMotorController &rightFrontMotor, 
			PidMotorController &rightBackMotor
		);
		
		TankDriveSubsystem
		(
			PidMotorController &leftMotor,
			PidMotorController &rightMotor
		);
		
		TankDriveSubsystem
		(
			boost::shared_ptr<CANJaguar> leftFrontMotor, 
			boost::shared_ptr<CANJaguar> leftBackMotor,
			boost::shared_ptr<CANJaguar> rightFrontMotor, 
			boost::shared_ptr<CANJaguar> rightBackMotor
		);
		
		TankDriveSubsystem
		(
			boost::shared_ptr<CANJaguar> leftMotor,
			boost::shared_ptr<CANJaguar> rightMotor
		);
		
		TankDriveSubsystem
		(
			CANJaguar *leftFrontMotor, 
			CANJaguar *leftBackMotor,
			CANJaguar *rightFrontMotor, 
			CANJaguar *rightBackMotor,
			bool canDeleteMotors = false
		);
		
		TankDriveSubsystem
		(
			CANJaguar *leftMotor, 
			CANJaguar *rightMotor,
			bool canDeleteMotors = false
		);
		
		TankDriveSubsystem
		(
			CANJaguar &leftFrontMotor, 
			CANJaguar &leftBackMotor,
			CANJaguar &rightFrontMotor, 
			CANJaguar &rightBackMotor
		);
		
		TankDriveSubsystem
		(
			CANJaguar &leftMotor, 
			CANJaguar &rightMotor
		);
		
		TankDriveSubsystem
		(
			SpeedController *leftFrontMotor, 
			SpeedController *leftBackMotor,
			SpeedController *rightFrontMotor, 
			SpeedController *rightBackMotor
		);
		
		TankDriveSubsystem
		(
			SpeedController *leftMotor,
			SpeedController *rightMotor
		);
		
		TankDriveSubsystem
		(
			SpeedController &leftFrontMotor, 
			SpeedController &leftBackMotor,
			SpeedController &rightFrontMotor, 
			SpeedController &rightBackMotor
		);
		
		TankDriveSubsystem
		(
			SpeedController &leftMotor,
			SpeedController &rightMotor
		);
		
		virtual ~TankDriveSubsystem();
		
		UINT32 GetMotorRankCount() const;
		UINT32 GetMotorCount() const;
		
		void DriveByTank(double leftValue, double rightValue);
		void DriveByMagnitudeAndCurve(double magnitude, double curve);
		
		boost::shared_ptr<Operation<bool> > DriveDistanceByTank(TankValue value, double threshold);
		
		inline boost::shared_ptr<Operation<bool> > DriveDistanceByTank(TankValue value)
		{
			return DriveDistanceByTank(value, GetDistanceThreshold());
		}
		
		inline boost::shared_ptr<Operation<bool> > DriveDistanceByTank(double left, double right, double threshold)
		{
			return DriveDistanceByTank(TankValue(left, right), threshold);
		}
		
		inline boost::shared_ptr<Operation<bool> > DriveDistanceByTank(double left, double right)
		{
			return DriveDistanceByTank(left, right, GetDistanceThreshold());
		}
		
		void SetControlMode(ControlMode controlMode);
		ControlMode GetControlMode();
		void SetNeutralMode(NeutralMode neutralMode);
		
		bool GetMotorInvertation(MotorPosition motorPosition) const;
		
		void SetMotorInvertations
		(
			bool leftFrontIsInverted, 
			bool leftBackIsInverted, 
			bool rightFrontIsInverted, 
			bool rightBackIsInverted
		);
		
		void SetMotorInvertations(bool leftIsInverted, bool rightIsInverted);
		void SetMotorInvertation(MotorPosition motorPosition, bool isInverted);
		void SetMotorInvertation(Side side, UINT32 rank, bool isInverted);
		
		void SetEncodersCodesPerRevolution(UINT16 codesPerRevolution);
		
		void SetEncodersCodesPerRevolution
		(
			UINT16 leftFrontCodesPerRevolution, 
			UINT16 leftBackCodesPerRevolution, 
			UINT16 rightFrontCodesPerRevolution, 
			UINT16 rightBackCodesPerRevolution
		);
		
		void SetEncoderCodesPerRevolution
		(
			MotorPosition motorPosition, 
			UINT16 codesPerRevolution
		);
		
		void SetEncoderCodesPerRevolution
		(
			Side side,
			UINT32 rank,
			UINT16 codesPerRevolution
		);
		
		void SetPIDs(ControlMode controlMode, double p, double i, double d);
		
		void SetPID
		(
			ControlMode controlMode,
			MotorPosition motorPosition,
			double p,
			double i,
			double d
		);
		
		void SetMaximumSpeed(double maximumSpeed);
		void SetMaximumVoltage(double maximumVoltage);
		void SetMaximumCurrent(double maximumCurrent);
		
		double GetDistanceThreshold();
		void SetDistanceThreshold(double distanceThreshold);
		
		double GetExpiration();
		void SetExpiration(double expiration);
	protected:
		typedef
			std::map<MotorPosition, boost::shared_ptr<PidMotorController> >::iterator
			MotorIterator;
		
		static double GetMotorInvertationMultiplier(bool isInverted);
		
		bool m_reversed;
		bool m_ownsDrive;
		double m_maxOutput;
		double m_maxSpeed;
		double m_maxVoltage;
		double m_maxCurrent;
		
		bool m_usesPidEncoderMotors;
		
		ControlMode m_controlMode;
		int m_counterPeriods;
		
		MotorIterator m_motorCanidateForCheck;
		
		bool m_driveUpdated;
		MotorSafetyHelper *m_safetyHelper;
		
		// These are functions inherited from RobotSubsystem.
		virtual void HandlePeriodic();
		virtual void HandleAwake();
		virtual void HandleStop();
		
		void SetMaximumOutput(double maximumOutput);
		double GetMotorInvertationMultiplier(MotorPosition position) const;
		double GetMotorInvertationMultiplier(Side side, UINT32 rank) const;
		PidMotorController &GetPidMotorController(MotorPosition position);
		PidMotorController &GetPidMotorController(Side side, UINT32 rank);
		void FeedSafety();
		//void PrintFaults();
	private:
		struct PidValue
		{
			public:
				PidValue();
				PidValue(double p, double i, double d);
				
				double m_p;
				double m_i;
				double m_d;
		};
		
		class DriveDistanceOperation :
			public Operation<bool>
		{
			public:
				DriveDistanceOperation(TankDriveSubsystem &drive, const TankValue &value, double threshold);
				
				virtual void HandleLaunch();
				virtual void HandlePeriodic();
				virtual bool HandleCancel();
			private:
				TankDriveSubsystem &m_drive;
				TankValue m_value;
				double m_threshold;
				ControlMode m_controlModePrevious;
		};
		
		RobotDrive &m_drive;
		
		const UINT32 m_motorRankCount;
		std::map<MotorPosition, boost::shared_ptr<PidMotorController> > m_motors;
		std::map<MotorPosition, bool> m_motorInvertations;
		std::map<ControlMode, std::map<MotorPosition, PidValue> > m_pidValues;
		
		double m_distanceThreshold;
		
		void SetPidMotorController(MotorPosition position, boost::shared_ptr<PidMotorController> motor);
		void SetPidMotorController(Side side, UINT32 rank, boost::shared_ptr<PidMotorController> motor);
		void SetPidMotorController(Side side, UINT32 rank, PidMotorController *motor, bool canDeleteMotor = false);
		void SetPidMotorController(Side side, UINT32 rank, PidMotorController &motor);
		void SetPidMotorController(Side side, UINT32 rank, CANJaguar *motor, bool canDeleteMotor = false);
		void SetPidMotorController(Side side, UINT32 rank, CANJaguar &motor);
		void Startup();
};

class ArcadeDriveService :
	public JoystickDriveService
	<
		TankDriveSubsystem,
		TankValue,
		TankDriveSubsystem::DriveByTank
	>
{
	public:
		ArcadeDriveService(TankDriveSubsystem &drive, GenericHID &joystick) :
			JoystickDriveService
			<
				TankDriveSubsystem,
				TankValue,
				TankDriveSubsystem::DriveByTank
			>(drive, new ArcadeDriveFilter(), joystick, true)
		{
		}
};

class TankDriveService :
	public TwoJoystickDriveService
	<
		TankDriveSubsystem,
		TankValue,
		TankDriveSubsystem::DriveByTank
	>
{
	public:
		TankDriveService(TankDriveSubsystem &drive, GenericHID &leftJoystick, GenericHID &rightJoystick) :
			TwoJoystickDriveService
			<
				TankDriveSubsystem,
				TankValue,
				TankDriveSubsystem::DriveByTank
			>(drive, new TankDriveFilter(), leftJoystick, rightJoystick, true)
		{
		}
};

#endif // #ifndef FOURMOTORDRIVESUBSYSTEM_H 
